/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <cmath>
#include <ctime>
#include <memory>
#include <thread>
#include <vector>

#include "absl/strings/str_cat.h"

#include "cyber/cyber.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/util/message_util.h"
#include "modules/drivers/gnss/proto/config.pb.h"
#include "modules/drivers/gnss/stream/raw_stream.h"
#include "modules/drivers/gnss/stream/stream.h"

namespace apollo {
namespace drivers {
namespace gnss {


void switch_stream_status(const apollo::drivers::gnss::Stream::Status &status,
                          StreamStatus_Type *report_status_type) {
  switch (status) {
    case apollo::drivers::gnss::Stream::Status::CONNECTED:
      *report_status_type = StreamStatus::CONNECTED;
      break;

    case apollo::drivers::gnss::Stream::Status::DISCONNECTED:
      *report_status_type = StreamStatus::DISCONNECTED;
      break;

    case apollo::drivers::gnss::Stream::Status::ERROR:
    default:
      *report_status_type = StreamStatus::DISCONNECTED;
      break;
  }
}
std::string getLocalTimeFileStr(const std::string &gpsbin_folder) {
  time_t it = std::time(0);
  char local_time_char[64];
  std::tm time_tm;
  localtime_r(&it, &time_tm);

  std::strftime(local_time_char, sizeof(local_time_char), "%Y%m%d_%H%M%S",
                &time_tm);
  std::string local_time_str = local_time_char;
  ACHECK(cyber::common::EnsureDirectory(gpsbin_folder))
      << "gbsbin folder : " << gpsbin_folder << " create fail";
  std::string local_time_file_str =
      gpsbin_folder + "/" + local_time_str + ".bin";
  return local_time_file_str;
}

Stream *create_stream(const config::Stream &sd) {
  switch (sd.type_case()) {
    case config::Stream::kSerial:
      if (!sd.serial().has_device()) {
        AERROR << "Serial def has no device field.";
        return nullptr;
      }
      if (!sd.serial().has_baud_rate()) {
        AERROR << "Serial def has no baud_rate field. Use default baud rate "
               << sd.serial().baud_rate();
        return nullptr;
      }
      return Stream::create_serial(sd.serial().device().c_str(),
                                   sd.serial().baud_rate());

    default:
      return nullptr;
  }
}

RawStream::RawStream(const config::Config &config,
                     const std::shared_ptr<apollo::cyber::Node> &node)
    : config_(config), node_(node) {
  data_parser_ptr_.reset(new DataParser(config_, node_));
}

RawStream::~RawStream() {
  // this->Logout();
  this->Disconnect();
  if (gpsbin_stream_ != nullptr) {
    gpsbin_stream_->close();
  }
  if (data_thread_ptr_ != nullptr && data_thread_ptr_->joinable()) {
    data_thread_ptr_->join();
  }
}

bool RawStream::Init() {
  CHECK_NOTNULL(data_parser_ptr_);
  if (!data_parser_ptr_->Init()) {
    AERROR << "Init data parser failed.";
    return false;
  }
  stream_status_.set_ins_stream_type(StreamStatus::DISCONNECTED);
  stream_writer_ = node_->CreateWriter<StreamStatus>(FLAGS_stream_status_topic);

  common::util::FillHeader("gnss", &stream_status_);
  stream_writer_->Write(stream_status_);

  // Creates streams.
  Stream *s = nullptr;
  if (!config_.has_data()) {
    AINFO << "Error: Config file must provide the data stream.";
    return false;
  }
  s = create_stream(config_.data());
  if (s == nullptr) {
    AERROR << "Failed to create data stream.";
    return false;
  }
  data_stream_.reset(s);

  Status *status = new Status();
  if (!status) {
    AERROR << "Failed to create data stream status.";
    return false;
  }
  data_stream_status_.reset(status);

  // connect 
  if (!Connect()) {
    AERROR << "gnss driver connect failed.";
    return false;
  }

  const std::string gpsbin_file = getLocalTimeFileStr(config_.gpsbin_folder());
  gpsbin_stream_.reset(new std::ofstream(
      gpsbin_file, std::ios::app | std::ios::out | std::ios::binary));
  stream_writer_ = node_->CreateWriter<StreamStatus>(FLAGS_stream_status_topic);
  raw_writer_ = node_->CreateWriter<RawData>(FLAGS_gnss_raw_data_topic);
  cyber::ReaderConfig reader_config;
  reader_config.channel_name = FLAGS_gnss_raw_data_topic;
  reader_config.pending_queue_size = 100;
  gpsbin_reader_ = node_->CreateReader<RawData>(
      reader_config, [&](const std::shared_ptr<RawData> &raw_data) {
        GpsbinCallback(raw_data);
      });

  return true;
}

void RawStream::Start() {
  data_thread_ptr_.reset(new std::thread(&RawStream::DataSpin, this));
}


bool RawStream::Connect() {
  if (data_stream_) {
    if (data_stream_->get_status() != Stream::Status::CONNECTED) {
      if (!data_stream_->Connect()) {
        AERROR << "data stream connect failed.";
        return false;
      }
      data_stream_status_->status = Stream::Status::CONNECTED;
      stream_status_.set_ins_stream_type(StreamStatus::CONNECTED);
    }
  }
  return true;
}

bool RawStream::Disconnect() {
  if (data_stream_) {
    if (data_stream_->get_status() == Stream::Status::CONNECTED) {
      if (!data_stream_->Disconnect()) {
        AERROR << "data stream disconnect failed.";
        return false;
      }
    }
  }

  return true;
}

void RawStream::StreamStatusCheck() {
  bool status_report = false;
  StreamStatus_Type report_stream_status;

  if (data_stream_ &&
      (data_stream_->get_status() != data_stream_status_->status)) {
    data_stream_status_->status = data_stream_->get_status();
    status_report = true;
    switch_stream_status(data_stream_status_->status, &report_stream_status);
    stream_status_.set_ins_stream_type(report_stream_status);
  }

  if (status_report) {
    common::util::FillHeader("gnss", &stream_status_);
    stream_writer_->Write(stream_status_);
  }
}

void RawStream::DataSpin() {
  common::util::FillHeader("gnss", &stream_status_);
  stream_writer_->Write(stream_status_);
  while (cyber::OK()) {
    size_t length = data_stream_->read(buffer_, BUFFER_SIZE);
    if (length == BUFFER_SIZE) {
      std::shared_ptr<RawData> msg_pub = std::make_shared<RawData>();
      if (!msg_pub) {
        AERROR << "New data sting msg failed.";
        continue;
      }
      msg_pub->set_data(reinterpret_cast<const char *>(buffer_), length);
      raw_writer_->Write(msg_pub);
      data_parser_ptr_->ParseRawData(msg_pub->data());
    }
    StreamStatusCheck();
  }
}

void RawStream::GpsbinCallback(const std::shared_ptr<RawData const> &raw_data) {
  if (gpsbin_stream_ == nullptr) {
    return;
  }
  gpsbin_stream_->write(raw_data->data().c_str(), raw_data->data().size());
}

}  // namespace gnss
}  // namespace drivers
}  // namespace apollo
